#pragma once
#include "Vector3.h"

enum FUNCTION_TYPE
{
	FUNCTION_FWD_EULER,
	FUNCTION_VERLET_CLASSIC,
	FUNCTION_RK4,
};

template <class T>
struct state_functorPV
{
	state_functorPV()
	{
		position = Vector3<T>::origin;
		velocity = Vector3<T>::origin;
		acceleration = Vector3<T>::origin;
	}
	state_functorPV( const Vector3<T>& p, const Vector3<T>& v, const Vector3<T>& acc )
	{
		position = p;
		velocity = v;
		acceleration = acc;
	}
	state_functorPV( const state_functorPV& cp )
	{
		position = cp.position;
		velocity = cp.velocity;
		acceleration = cp.acceleration;
	}

	const state_functorPV<T>& operator=(const state_functorPV<T>& rhs)
	{
		position = rhs.position;
		velocity = rhs.velocity;
		acceleration = rhs.acceleration;

		return (*this);
	}

	state_functorPV<T> operator+(const state_functorPV<T>& rhs)
	{
		state_functorPV result;
		result.position = position + rhs.position;
		result.velocity = velocity + rhs.velocity;

		return result;
	}

	const state_functorPV<T>& operator+=(const state_functorPV<T>& rhs)
	{
		position += rhs.position;
		velocity += rhs.velocity;

		return (*this);
	}

	state_functorPV<T> operator*(T scalar)
	{
		state_functorPV result;
		result.position = position * scalar;
		result.velocity = velocity * scalar;
		return result;
	}

	Vector3<T> acceleration;
	Vector3<T> velocity;
	Vector3<T> position;
};

template <class T>
class FunctionPV
{
public:
	FunctionPV(void) {};
	~FunctionPV(void) {};
	
	inline state_functorPV<T> operator()(T t, const state_functorPV<T>& s) const
	{
		state_functorPV<T> result(s);
		result.position = s.velocity + s.acceleration*t;
		result.velocity = s.acceleration;
		return result;
	}
};

template <class T>
void NumericIntegral(T t, T dt, state_functorPV<T>& s, FUNCTION_TYPE type)
{

	switch (type)
	{
	case FUNCTION_VERLET_CLASSIC:
		s.velocity += s.acceleration*dt;
		s.position += s.velocity*dt;
		break;
	case FUNCTION_RK4:
		{
			FunctionPV<T> f;
			state_functorPV<T> y1,y2,y3,y4,f1,f2,f3,f4;
			y1 = s;
			f1 = f(0,y1);
			y2 = s + f1*(dt/2);
			f2 = f(dt/2,y2);
			y3 = s + f2*(dt/2);
			f3 = f(dt/2,y3);
			y4 = s + f3*(dt);
			f4 = f(dt,y4);
			s += ((f1+f2*2+f3*2+f4)*(dt/6));
		}
		break;
		// using fwd euler for default
	case FUNCTION_FWD_EULER:
	default:
		s.position += s.velocity*dt;
		s.velocity += s.acceleration*dt;
		break;
	}


	//result.x = 10*(s.y - s.x);
	//result.y = (s.x*(28 - s.z) - s.y);
	//result.z = s.x*s.y - 8.0*s.z/3;
}